#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "pcl_conversions/pcl_conversions.h"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/make_shared.h>

#include <chrono>
#include <string>
#include <memory>

class PointCloudPub : public rclcpp::Node
{
private:
    using PointT = pcl::PointXYZI;
    using PointCloud = pcl::PointCloud<PointT>;
    using PointCloudMsg = sensor_msgs::msg::PointCloud2;
    PointCloudMsg raw_msg_;
    PointCloudMsg filtered_msg_;

    rclcpp::Publisher<PointCloudMsg>::SharedPtr cloud_pub_raw_;
    rclcpp::Publisher<PointCloudMsg>::SharedPtr cloud_pub_filtered_;
    rclcpp::TimerBase::SharedPtr timer_;
    PointCloud::Ptr raw_cloud_;
    PointCloud::Ptr filtered_cloud_;

    std::string frame_id_;

    void timer_callback()
    {
        raw_msg_.header.stamp = this->now();
        cloud_pub_raw_->publish(raw_msg_);
        
        filtered_msg_.header.stamp = this->now();
        cloud_pub_filtered_->publish(filtered_msg_);
    }

public:
    explicit PointCloudPub(const std::string &name) : Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "节点 %s: 已启动.", name.c_str());

        this->declare_parameter<std::string>("raw_cloud_pcd_file", "");
        std::string raw_file = this->get_parameter("raw_cloud_pcd_file").as_string();
        
        this->declare_parameter<std::string>("filtered_cloud_pcd_file", "");
        std::string filtered_file = this->get_parameter("filtered_cloud_pcd_file").as_string();

        this->declare_parameter<int>("interval_time_ms", 200);
        int interval_time_ms = this->get_parameter("interval_time_ms").as_int();

        this->declare_parameter<std::string>("frame_id", "base_link");
        frame_id_ = this->get_parameter("frame_id").as_string();

        RCLCPP_INFO(this->get_logger(), "原始pcd文件: %s", raw_file.c_str());
        RCLCPP_INFO(this->get_logger(), "滤波后pcd文件: %s", filtered_file.c_str());
        RCLCPP_INFO(this->get_logger(), "发布间隔时间 (ms): %d", interval_time_ms);
        RCLCPP_INFO(this->get_logger(), "发布坐标系 (frame_id): %s", frame_id_.c_str());

        if (raw_file.empty() || filtered_file.empty())
        {
            RCLCPP_ERROR(this->get_logger(), "PCD文件路径参数未设置，节点将关闭。");
            return;
        }
        
        raw_cloud_ = pcl::make_shared<PointCloud>();
        filtered_cloud_ = pcl::make_shared<PointCloud>();

        if (pcl::io::loadPCDFile<PointT>(raw_file, *raw_cloud_) == -1)
        {
            RCLCPP_ERROR(this->get_logger(), "原始pcd文件读取错误: %s", raw_file.c_str());
            return;
        }
        RCLCPP_INFO(this->get_logger(), "原始点云点数: %ld", raw_cloud_->size());
        
        if (pcl::io::loadPCDFile<PointT>(filtered_file, *filtered_cloud_) == -1)
        {
            RCLCPP_ERROR(this->get_logger(), "滤波后pcd文件读取错误: %s", filtered_file.c_str());
            return;
        }
        RCLCPP_INFO(this->get_logger(), "滤波后点云点数: %ld", filtered_cloud_->size());

        pcl::toROSMsg(*raw_cloud_, raw_msg_);
        raw_msg_.header.frame_id = frame_id_;

        pcl::toROSMsg(*filtered_cloud_, filtered_msg_);
        filtered_msg_.header.frame_id = frame_id_;
        cloud_pub_raw_ = this->create_publisher<PointCloudMsg>("/raw_pointcloud", rclcpp::QoS(10));
        cloud_pub_filtered_ = this->create_publisher<PointCloudMsg>("/filtered_pointcloud", rclcpp::QoS(10));
        
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(interval_time_ms), 
            std::bind(&PointCloudPub::timer_callback, this));        
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PointCloudPub>("pointcloud_pub");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}